#include "MyGLDrawer.h"
#include <iostream>
#include <cmath>

using namespace std;

MyGLDrawer::MyGLDrawer(QWidget *parent)
	: QGLWidget(parent)
{
//	setFormat(QGLFormat(DoubleBuffer | DepthBuffer));
	rotX = 0;
	rotY = 0;
	rotZ = 0;

	uint M, N, C, U;
	M = 8;			// Number of images
	N = 12; 		// Number of object points
	C = 1;			// Number of cameras
	U = 7*M+3*C;		// Number of unkowns

	solver = new ntpg(M,N,C);
	initPhotogram();

	rng = 25.0;
}

MyGLDrawer::~MyGLDrawer()
{
	if(solver)
		delete solver;
}

void MyGLDrawer::initializeGL()
{
	// Set up the rendering context, define display lists etc.:
	glClearColor(0.0, 0.0, 0.0, 0.0);
	glShadeModel(GL_FLAT);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_CULL_FACE);
}

void MyGLDrawer::resizeGL(int w, int h)
{
	// setup viewport, projection etc.:
	double aspect = (double)w/(double)h;
	glViewport(0, 0, (GLint)w, (GLint)h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	if(aspect>=1.0)
		glOrtho(-rng*aspect, rng*aspect, -rng, rng, -rng, rng);
	else
		glOrtho(-rng, rng, -rng/aspect, rng/aspect, -rng, rng);
	glMatrixMode(GL_MODELVIEW);
}

void MyGLDrawer::paintGL()
{
	// draw the scene:
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(0.0, 0.0, 0.0);
	glRotatef(rotX, 1.0, 0.0, 0.0);
	glRotatef(rotY, 0.0, 1.0, 0.0);
	glRotatef(rotZ, 0.0, 0.0, 1.0);
	draw();
}

void MyGLDrawer::mousePressEvent(QMouseEvent *event)
{
	lastPos = event->pos();
}

void MyGLDrawer::mouseMoveEvent(QMouseEvent *event)
{
	GLfloat dx = (GLfloat)(event->x() - lastPos.x()) / width();
	GLfloat dy = (GLfloat)(event->y() - lastPos.y()) / width();

	if (event->buttons() & Qt::LeftButton) {
		rotX += 180 * dy;
		rotY += 180 * dx;
		updateGL();
	} else if (event->buttons() & Qt::RightButton) {
		rotZ += 180 * dx;
		rotY += 180 * dy;
		updateGL();
	}
	lastPos = event->pos();
}

void MyGLDrawer::mouseDoubleClickEvent(QMouseEvent *event)
{

}

void MyGLDrawer::drawAxes()
{
	glLineWidth(1.0);
	glBegin(GL_LINES);
		glColor3d(1.0, 0.0, 0.0);
		glVertex3d(0.0, 0.0, 0.0);
		glVertex3d(rng/4.0, 0.0, 0.0);

		glColor3d(0.0, 1.0, 0.0);
		glVertex3d(0.0, 0.0, 0.0);
		glVertex3d(0.0, rng/4.0, 0.0);

		glColor3d(0.0, 0.0, 1.0);
		glVertex3d(0.0, 0.0, 0.0);
		glVertex3d(0.0, 0.0, rng/4.0);
	glEnd();
}

void MyGLDrawer::drawObjectPoints()
{
	glColor3d(1.0, 1.0, 1.0);
	glPointSize(5.0);
	glBegin(GL_POINTS);
	glVertex3d(solver->rx(1), solver->ry(1), solver->rz(1));
	glEnd();
	glColor3d(1.0, 0.0, 0.0);
	glPointSize(4.0);
	glBegin(GL_POINTS);
	for(uint n=2; n<=solver->getN(); n++)
		glVertex3d(solver->rx(n), solver->ry(n), solver->rz(n));
	glEnd();
}

void MyGLDrawer::drawImagePoints(uint m)
{
	ftype x,y,z,ux,uy;
	glColor3d(1.0, 1.0, 1.0);

	glPointSize(2.0);
	ux = solver->ux(m,1);
	uy = solver->uy(m,1);
	solver->globalImagePoint(m,ux,uy,x,y,z);

	glBegin(GL_POINTS);
		glVertex3d(x,y,z);
	glEnd();

	glPointSize(1.0);
	glBegin(GL_POINTS);
	for(uint n=2; n<=solver->getN(); n++) {
		ux = solver->ux(m,n);
		uy = solver->uy(m,n);
		solver->globalImagePoint(m,ux,uy,x,y,z);
		glVertex3d(x,y,z);
	}
	glEnd();
}

void MyGLDrawer::drawCamera(uint k)
{

}

void MyGLDrawer::drawImage(uint m)
{
	ftype x0, y0, z0;
	ftype x1, y1, z1;
	ftype x2, y2, z2;
	ftype x3, y3, z3;
	solver->globalImagePixel(m,0,0,x0,y0,z0);
	solver->globalImagePixel(m,1600,0,x1,y1,z1);
	solver->globalImagePixel(m,0,1200,x2,y2,z2);
	solver->globalImagePixel(m,1600,1200,x3,y3,z3);

	glPointSize(3.0);
	glBegin(GL_POINTS);
		glColor3d(0.0, 1.0, 0.0);
		glVertex3d(solver->tx(m), solver->ty(m), solver->tz(m));
	glEnd();

	glLineWidth(2.0);
	glColor3d(0.5, 0.5, 0.5);
	glBegin(GL_LINES);
		glColor3d(1.0, 0.0, 0.0);
		glVertex3d(x0,y0,z0);
		glVertex3d(x1,y1,z1);
		glColor3d(0.0, 1.0, 0.0);
		glVertex3d(x0,y0,z0);
		glVertex3d(x2,y2,z2);
		glColor3d(0.5, 0.5, 0.5);
		glVertex3d(x1,y1,z1);
		glVertex3d(x3,y3,z3);
		glVertex3d(x3,y3,z3);
		glVertex3d(x2,y2,z2);
	glEnd();
}

void MyGLDrawer::draw()
{
	drawAxes();
	drawObjectPoints();
	uint m;
	for(m=1; m<=solver->getM(); m++) {
		drawImage(m);
		drawImagePoints(m);
	}
}

void MyGLDrawer::initPhotogram()
{
	// for every image m set camera c = 1..C
	// there was only one camera used in this experiment
	solver->setk(1,1);	// set image 1 to camera 1
	solver->setk(2,1);  	// set image 2 to camera 1
	solver->setk(3,1);  	// set image 3 to camera 1
	solver->setk(4,1);  	// set image 4 to camera 1
	solver->setk(5,1);  	// set image 5 to camera 1
	solver->setk(6,1);  	// set image 6 to camera 1
	solver->setk(7,1);  	// set image 7 to camera 1
	solver->setk(8,1);  	// set image 8 to camera 1

//*
	solver->setfocalplaneresolution(7766.99, 7741.94);
    solver->setv(1, 0.101579, 0.0698737, -0.231195, 1);

    solver->sett(1, 3.18179,1.5339, 12.6036, 1);
    solver->sett(2, 4.0253,2.09677, 14.2535, 1);
    solver->sett(3, 2.42264,-4.26432, 11.6891, 1);
    solver->sett(4, 3.44562,-4.35096, 12.9042, 1);
    solver->sett(5, -5.52768,0.626302, 14.343, 1);
    solver->sett(6, -7.45597,0.323494, 14.541, 1);
    solver->sett(7, 12.3683,-1.27858, 12.9239, 1);
    solver->sett(8, 13.121,-0.677504, 13.1503, 1);

    solver->setq(1, -0.0317717, -0.999182, -0.00431594, 0.0246601, 1);
    solver->setq(2, -0.00684014, -0.704817, 0.708627, 0.0321653, 1);
    solver->setq(3, 0.22395, 0.974568, -0.00777824, 0.00169006, 1);
    solver->setq(4, 0.142256, 0.699432, -0.685285, -0.144715, 1);
    solver->setq(5, 0.206306, 0.69437, -0.67507, 0.139888, 1);
    solver->setq(6, -0.0887803, -0.952461, 0.036764, -0.289111, 1);
    solver->setq(7, -0.10638, -0.943131, 0.0093228, 0.314801, 1);
    solver->setq(8, 0.260941, 0.702805, 0.630229, -0.201958, 1);
//*/

/*
	// for every camera c = 1..C set camera parameters
	// note: this information was obtained in the meta data from jpg file
	solver->setfocalplaneresolution(7766.99, 7741.94);
	ftype xres = solver->getXfocalplaneresolution();
	ftype yres = solver->getYfocalplaneresolution();
	solver->setv(1, 1600.0/xres*0.5, 1200.0/yres*0.5, -0.1968, 0);
	// for every image m set image position t[1..M]
	solver->sett(1, 3.0, 1.0, 24.0, 0);
	solver->sett(2, 3.0, 1.0, 24.0, 0);
	solver->sett(3, 3.0, -12.0, 18.0, 0);
	solver->sett(4, 3.0, -12.0, 18.0, 0);
	solver->sett(5, -12.0, 1.0, 18.0, 0);
	solver->sett(6, -12.0, 1.0, 18.0, 0);
	solver->sett(7, 12.0, 1.0, 18.0, 0);
	solver->sett(8, 12.0, 1.0, 18.0, 0);

	// for every image m set image orientation
	solver->orientImage(1, -solver->tx(1), -solver->ty(1), -solver->tz(1), 90.0, 0);
	solver->orientImage(2, -solver->tx(2), -solver->ty(2), -solver->tz(2), 180.0, 0);
	solver->orientImage(3, -solver->tx(3), -solver->ty(3), -solver->tz(3), 0.0, 0);
	solver->orientImage(4, -solver->tx(4), -solver->ty(4), -solver->tz(4), 90.0, 0);
	solver->orientImage(5, -solver->tx(5), -solver->ty(5), -solver->tz(5), 180.0, 0);
	solver->orientImage(6, -solver->tx(6), -solver->ty(6), -solver->tz(6), 90.0, 0);
	solver->orientImage(7, -solver->tx(7), -solver->ty(7), -solver->tz(7), 90.0, 0);
	solver->orientImage(8, -solver->tx(8), -solver->ty(8), -solver->tz(8), 0.0, 0);
//*/
/*
	// set object points
	solver->setr(1, 0.0, 0.0, 0.0, 1);
	solver->setr(2, 2.0, 0.0, 0.0, 1);
	solver->setr(3, 4.0, 0.0, 0.0, 1);
	solver->setr(4, 6.0, 0.0, 0.0, 1);
	solver->setr(5, 0.0, 2.0, 0.0, 1);
	solver->setr(6, 2.0, 2.0, 0.0, 1);
	solver->setr(7, 4.0, 2.0, 0.0, 1);
	solver->setr(8, 6.0, 2.0, 0.0, 1);
	solver->setr(9, 0.0, 4.0, 0.0, 1);
	solver->setr(10, 2.0, 4.0, 0.0, 1);
	solver->setr(11, 4.0, 4.0, 0.0, 1);
	solver->setr(12, 6.0, 4.0, 0.0, 1);
//*/

//	solver->setrx(1, 1.0, 0);
//	solver->setry(1, 1.0, 0);
//	solver->setrz(1, 1.0, 0);
	solver->setr(1, 0.0, 0.0, 0.0, 0);
	solver->setr(2, 0.0, 0.0, 0.0, 0);
	solver->setr(3, 0.0, 0.0, 0.0, 0);
	solver->setr(4, 0.0, 0.0, 0.0, 0);
	solver->setr(5, 0.0, 0.0, 0.0, 0);
	solver->setr(6, 0.0, 0.0, 0.0, 0);
	solver->setr(7, 0.0, 0.0, 0.0, 0);
	solver->setr(8, 0.0, 0.0, 0.0, 0);
	solver->setr(9, 0.0, 0.0, 0.0, 0);
	solver->setr(10, 0.0, 0.0, 0.0, 0);
	solver->setr(11, 0.0, 0.0, 0.0, 0);
	solver->setr(12, 0.0, 0.0, 0.0, 0);


//	solver->setr(1, 0.0, 0.0, 0.0, 1);
//	solver->setr(2, 2.0, 0.0, 0.0, 1);
//	solver->setr(3, 4.0, 0.0, 0.0, 1);
//	solver->setr(4, 6.0, 0.0, 0.0, 1);
//	solver->setr(5, 0.0, 2.0, 0.0, 1);
//	solver->setr(6, 2.0, 2.0, 0.0, 1);
//	solver->setr(7, 4.0, 2.0, 0.0, 1);
//	solver->setr(8, 6.0, 2.0, 0.0, 1);
//	solver->setr(9, 0.0, 4.0, 0.0, 1);
//	solver->setr(10, 2.0, 4.0, 0.0, 1);
//	solver->setr(11, 4.0, 4.0, 0.0, 1);
//	solver->setr(12, 6.0, 4.0, 0.0, 1);


	// set image points
	// set image 1 points
	solver->setu(1, 1, 424, 869, 1);
	solver->setu(1, 2, 706, 876, 1);
	solver->setu(1, 3, 994, 881, 1);
	solver->setu(1, 4, 1285, 883, 1);
	solver->setu(1, 5, 429, 585, 1);
	solver->setu(1, 6, 709, 588, 1);
	solver->setu(1, 7, 996, 590, 1);
	solver->setu(1, 8, 1284, 592, 1);
	solver->setu(1, 9, 438, 307, 1);
	solver->setu(1, 10, 714, 305, 1);
	solver->setu(1, 11, 996, 305, 1);
	solver->setu(1, 12, 1280, 307, 1);

	// set image 2 points
	solver->setu(2, 1, 1153, 980, 1);
	solver->setu(2, 2, 1156, 731, 1);
	solver->setu(2, 3, 1157, 477, 1);
	solver->setu(2, 4, 1154, 222, 1);
	solver->setu(2, 5, 901, 979, 1);
	solver->setu(2, 6, 902, 731, 1);
	solver->setu(2, 7, 901, 479, 1);
	solver->setu(2, 8, 899, 226, 1);
	solver->setu(2, 9, 654, 976, 1);
	solver->setu(2, 10, 651, 731, 1);
	solver->setu(2, 11, 649, 481, 1);
	solver->setu(2, 12, 648, 231, 1);

	// set image 3 points
	solver->setu(3, 1, 427, 729, 1);
	solver->setu(3, 2, 717, 727, 1);
	solver->setu(3, 3, 1009, 722, 1);
	solver->setu(3, 4, 1297, 716, 1);
	solver->setu(3, 5, 447, 473, 1);
	solver->setu(3, 6, 718, 469, 1);
	solver->setu(3, 7, 991, 465, 1);
	solver->setu(3, 8, 1260, 462, 1);
	solver->setu(3, 9, 466, 251, 1);
	solver->setu(3, 10, 719, 245, 1);
	solver->setu(3, 11, 973, 242, 1);
	solver->setu(3, 12, 1224,240, 1);

	// set image 4 points
	solver->setu(4, 1, 929, 983, 1);
	solver->setu(4, 2, 937, 722, 1);
	solver->setu(4, 3, 943, 457, 1);
	solver->setu(4, 4, 947, 191, 1);
	solver->setu(4, 5, 693, 954, 1);
	solver->setu(4, 6, 697, 707, 1);
	solver->setu(4, 7, 702, 457, 1);
	solver->setu(4, 8, 707, 207, 1);
	solver->setu(4, 9, 483, 926, 1);
	solver->setu(4, 10, 485, 693, 1);
	solver->setu(4, 11, 489, 458, 1);
	solver->setu(4, 12, 495, 222, 1);

	// set image 5 points
	solver->setu(5, 1, 1039, 773, 1);
	solver->setu(5, 2, 1036, 563, 1);
	solver->setu(5, 3, 1033, 376, 1);
	solver->setu(5, 4, 1028, 210, 1);
	solver->setu(5, 5, 802, 770, 1);
	solver->setu(5, 6, 812, 562, 1);
	solver->setu(5, 7, 822, 377, 1);
	solver->setu(5, 8, 829, 213, 1);
	solver->setu(5, 9, 572, 765, 1);
	solver->setu(5, 10, 595, 561, 1);
	solver->setu(5, 11, 616, 378, 1);
	solver->setu(5, 12, 635, 216, 1);

	// set image 6 points
	solver->setu(6, 1, 601, 870, 1);
	solver->setu(6, 2, 787, 846, 1);
	solver->setu(6, 3, 950, 823, 1);
	solver->setu(6, 4, 1094, 803, 1);
	solver->setu(6, 5, 576, 649, 1);
	solver->setu(6, 6, 761, 639, 1);
	solver->setu(6, 7, 924, 629, 1);
	solver->setu(6, 8, 1069, 621, 1);
	solver->setu(6, 9, 553, 435, 1);
	solver->setu(6, 10, 737, 438, 1);
	solver->setu(6, 11, 899, 440, 1);
	solver->setu(6, 12, 1043, 443, 1);

	// set image 7 points
	solver->setu(7, 1, 570, 775, 1);
	solver->setu(7, 2, 729, 774, 1);
	solver->setu(7, 3, 913, 773, 1);
	solver->setu(7, 4, 1127, 770, 1);
	solver->setu(7, 5, 585, 577, 1);
	solver->setu(7, 6, 741, 561, 1);
	solver->setu(7, 7, 921, 543, 1);
	solver->setu(7, 8, 1131, 523, 1);
	solver->setu(7, 9, 599, 387, 1);
	solver->setu(7, 10, 753, 358, 1);
	solver->setu(7, 11, 928, 326, 1);
	solver->setu(7, 12, 1132, 289, 1);

	// set image 8 points
	solver->setu(8, 1, 638, 335, 1);
	solver->setu(8, 2, 652, 483, 1);
	solver->setu(8, 3, 670, 654, 1);
	solver->setu(8, 4, 691, 853, 1);
	solver->setu(8, 5, 831, 324, 1);
	solver->setu(8, 6, 860, 469, 1);
	solver->setu(8, 7, 893, 637, 1);
	solver->setu(8, 8, 931, 832, 1);
	solver->setu(8, 9, 1019, 314, 1);
	solver->setu(8, 10, 1061, 457, 1);
	solver->setu(8, 11, 1108, 620, 1);
	solver->setu(8, 12, 1162, 810, 1);

	solver->solve(1.0e-10);
//	solver->showR();
//	solver->showQuats();
//	solver->showV();

}
